/*
 * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
 *
 * Copyright 2007-2012 VTT Technical Research Centre of Finland
 *
 * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
 *          <http://www.vtt.fi/multimedia/alvar.html>
 *
 * ALVAR is free software; you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with ALVAR; if not, see
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
 */

#include "ar_track_alvar/MultiMarkerInitializer.h"
#include <Eigen/StdVector>

using namespace std;

namespace alvar
{
using namespace std;

MultiMarkerInitializer::MultiMarkerInitializer(std::vector<int>& indices,
                                               int _filter_buffer_min,
                                               int _filter_buffer_max)
  : MultiMarker(indices), filter_buffer_min(_filter_buffer_min)
{
  marker_detected.resize(indices.size());
  pointcloud_filtered = new FilterMedian[indices.size() * 4 * 3];
  for (size_t i = 0; i < indices.size() * 4 * 3; i++)
  {
    pointcloud_filtered[i].setWindowSize(_filter_buffer_max);
  }

  MeasurementsReset();
}

void MultiMarkerInitializer::MeasurementsAdd(MarkerIterator& begin,
                                             MarkerIterator& end)
{
  // copy markers into measurements.
  vector<MarkerMeasurement, Eigen::aligned_allocator<MarkerMeasurement> >
      new_measurements;
  for (MarkerIterator& i = begin.reset(); i != end; ++i)
  {
    const Marker* marker = *i;
    int index = get_id_index(marker->GetId());
    if (index == -1)
      continue;
    MarkerMeasurement m;
    m.SetId(marker->GetId());
    m.SetMarkerSize(marker->GetMarkerEdgeLength(), marker->GetRes(),
                    marker->GetMargin());
    m.pose = marker->pose;
    m.marker_corners_img = i->marker_corners_img;
    new_measurements.push_back(m);
    marker_detected[index] = true;
  }

  // If we have seen the 0 marker the first time we push it into point could.
  for (MarkerIterator& i = begin.reset(); i != end; ++i)
  {
    const Marker* marker = *i;
    int id = marker->GetId();
    int index = get_id_index(id);
    // Initialize marker pose
    // if (id == 0 && marker_status[index] == 0)
    if (index == 0 && marker_status[index] == 0)
    {
      Pose pose;
      cv::Point3d corners[4];
      PointCloudCorners3d(marker->GetMarkerEdgeLength(), pose, corners);
      for (size_t j = 0; j < 4; ++j)
      {
        int p_index = pointcloud_index(id, j);
        pointcloud[p_index] = corners[j];
      }
      marker_status[index] = 1;
    }
  }

  measurements.push_back(new_measurements);
}

void MultiMarkerInitializer::MeasurementsReset()
{
  measurements.clear();
  PointCloudReset();
  fill(marker_status.begin(), marker_status.end(), 0);
  fill(marker_detected.begin(), marker_detected.end(), false);

  for (size_t i = 0; i < marker_indices.size() * 4 * 3; i++)
  {
    pointcloud_filtered[i].reset();
  }
}

int MultiMarkerInitializer::Initialize(Camera* cam)
{
  for (bool found_new = true; found_new;)
  {
    found_new = false;
    // Iterate through all measurements, try to compute Pose for each.
    for (MeasurementIterator mi = measurements.begin();
         mi != measurements.end(); ++mi)
    {
      vector<MarkerMeasurement, Eigen::aligned_allocator<MarkerMeasurement> >&
          markers = *mi;
      Pose pose;
      MarkerIteratorImpl<MarkerMeasurement> m_begin(markers.begin());
      MarkerIteratorImpl<MarkerMeasurement> m_end(markers.end());

      double err = _GetPose(m_begin, m_end, cam, pose);
      if (err >= 0)
      {
        // If pose is found, estimate marker poses for those that are still
        // unkown.
        found_new = updateMarkerPoses(markers, pose);
      }
    }
  }

  // Check if every marker has been seen
  int n_detected = 0;
  for (unsigned int i = 0; i < marker_indices.size(); ++i)
  {
    cout << i << " " << marker_detected[i] << " " << marker_status[i] << "\n";
    if (marker_detected[i] && marker_status[i] != 0)
      ++n_detected;
  }
  return n_detected;
}

bool MultiMarkerInitializer::updateMarkerPoses(
    vector<MarkerMeasurement, Eigen::aligned_allocator<MarkerMeasurement> >&
        markers,
    const Pose& pose)
{
  bool found_new = false;
  for (vector<MarkerMeasurement,
              Eigen::aligned_allocator<MarkerMeasurement> >::iterator i =
           markers.begin();
       i != markers.end(); ++i)
  {
    MarkerMeasurement& marker = *i;
    int id = marker.GetId();
    int index = get_id_index(id);
    if (index > 0 && !marker.globalPose)
    {
      // Compute absolute marker pose.
      double cam_posed[16];
      double mar_posed[16];
      cv::Mat cam_mat = cv::Mat(4, 4, CV_64F, cam_posed);
      cv::Mat mar_mat = cv::Mat(4, 4, CV_64F, mar_posed);
      pose.GetMatrix(cam_mat);
      marker.pose.GetMatrix(mar_mat);
      cam_mat = cam_mat.inv();
      mar_mat = cam_mat * mar_mat;
      marker.pose.SetMatrix(mar_mat);
      // Put marker into point cloud
      cv::Point3d corners[4];
      PointCloudCorners3d(marker.GetMarkerEdgeLength(), marker.pose, corners);
      for (size_t j = 0; j < 4; ++j)
      {
        cv::Point3d p;
        int p_index = pointcloud_index(id, j);
        p.x = pointcloud_filtered[3 * p_index + 0].next(corners[j].x);
        p.y = pointcloud_filtered[3 * p_index + 1].next(corners[j].y);
        p.z = pointcloud_filtered[3 * p_index + 2].next(corners[j].z);
        if (pointcloud_filtered[3 * p_index + 0].getCurrentSize() >=
            filter_buffer_min)
        {
          pointcloud[p_index] = p;
          marker_status[index] = 1;
        }
      }
      marker.globalPose = 1;
      found_new = true;
    }
  }
  return found_new;
}

}  // namespace alvar
